#!/usr/bin/env python3
# Shebang 行：指定使用 Python3 解释器执行此脚本
# 这确保了脚本在不同系统上都能使用正确的 Python 版本

# ==================== 模块导入部分 ====================
import time
# time 模块：提供时间相关的功能
# 在本代码中主要用于 sleep() 函数来模拟计算耗时

import rclpy
# rclpy：ROS2 的 Python 客户端库 (ROS Client Library for Python)
# 这是 ROS2 Python 开发的核心库，提供节点创建、通信等基础功能

from rclpy.node import Node
# Node 类：ROS2 节点的基类
# 所有 ROS2 节点都需要继承此类，它提供了日志记录、参数管理、通信等核心功能

from rclpy.action import ActionServer, CancelResponse, GoalResponse
# ActionServer：Action 服务器的实现类，用于创建和管理 Action 服务
# CancelResponse：取消请求的响应类型枚举 (ACCEPT/REJECT)
# GoalResponse：目标请求的响应类型枚举 (ACCEPT/REJECT)

from action_tutorials_interfaces.action import Fibonacci


# Fibonacci：自定义的 Action 接口定义
# 定义了 Action 的三个组成部分：Goal(目标)、Result(结果)、Feedback(反馈)
# 通常在 .action 文件中定义，编译后生成对应的 Python 类


# ==================== 主要服务器类定义 ====================
class FibonacciActionServer(Node):
    """
    斐波那契 Action 服务器类

    设计思路：
    1. 继承自 Node 类，获得 ROS2 节点的所有基础功能
    2. 封装 ActionServer 的创建和管理逻辑
    3. 实现三个关键回调函数来处理 Action 的完整生命周期
    """

    def __init__(self):
        """
        构造函数：初始化节点和 Action 服务器
        """
        # 调用父类构造函数，创建名为 'fibonacci_action_server' 的 ROS2 节点
        # 节点名称必须在 ROS2 网络中唯一，用于标识和通信
        super().__init__('fibonacci_action_server')

        # 创建 Action 服务器实例
        # 这是整个 Action 系统的核心组件
        self._action_server = ActionServer(
            self,  # 节点实例：ActionServer 需要绑定到特定节点
            Fibonacci,  # Action 类型：定义了通信的数据结构
            'fibonacci',  # Action 名称：客户端通过此名称找到服务器
            self.execute_callback,  # 执行回调：处理实际的任务逻辑
            goal_callback=self.goal_callback,  # 目标回调：决定是否接受新的目标请求
            cancel_callback=self.cancel_callback  # 取消回调：处理客户端的取消请求
        )

        # 记录服务器启动信息
        # get_logger() 返回与此节点关联的日志记录器
        self.get_logger().info('Fibonacci Action Server 已启动')

    def goal_callback(self, goal_request):
        """
        目标回调函数：处理客户端发送的新目标请求

        参数：
            goal_request: 包含客户端请求数据的目标对象
                         对于 Fibonacci Action，包含 order 字段（要计算的斐波那契数个数）

        返回：
            GoalResponse.ACCEPT 或 GoalResponse.REJECT

        设计考量：
        - 这是 Action 系统的"门卫"，决定哪些请求可以被处理
        - 可以在这里实现复杂的准入逻辑，如资源检查、权限验证等
        - 返回 REJECT 会立即拒绝请求，客户端会收到拒绝通知
        """
        # 记录收到的目标请求信息
        # goal_request.order 是客户端要求计算的斐波那契数的个数
        self.get_logger().info(f'收到目标请求: 计算前 {goal_request.order} 个斐波那契数')

        # 在这个简单示例中，我们接受所有目标请求
        # 实际应用中可能需要检查：
        # 1. 服务器当前是否繁忙
        # 2. 请求参数是否合理（如 order 是否在允许范围内）
        # 3. 客户端是否有足够权限
        return GoalResponse.ACCEPT

    def cancel_callback(self, goal_handle):
        """
        取消回调函数：处理客户端发送的取消请求

        参数：
            goal_handle: 目标句柄，代表要取消的特定目标

        返回：
            CancelResponse.ACCEPT 或 CancelResponse.REJECT

        设计考量：
        - 取消机制是 Action 相比 Service 的重要优势
        - 可以根据任务当前状态决定是否允许取消
        - 某些关键阶段可能不允许取消以确保数据一致性
        """
        # 记录取消请求
        self.get_logger().info('收到取消请求')

        # 在这个示例中，我们始终接受取消请求
        # 实际应用中可能需要考虑：
        # 1. 任务是否处于可以安全取消的状态
        # 2. 取消操作是否会造成资源泄漏
        # 3. 是否需要执行清理操作
        return CancelResponse.ACCEPT

    def execute_callback(self, goal_handle):
        """
        执行回调函数：实际执行任务的核心逻辑

        参数：
            goal_handle: 目标句柄，提供任务控制和通信接口
                        包含请求数据、状态管理、反馈发送等功能

        返回：
            Fibonacci.Result: 任务完成后的结果对象

        设计考量：
        - 这是 Action 系统的核心，实现具体的业务逻辑
        - 必须周期性检查取消请求以保持响应性
        - 通过反馈机制提供进度信息
        - 正确处理异常情况和资源清理
        """
        # 记录任务开始执行
        self.get_logger().info('开始执行斐波那契计算...')

        # 从目标句柄中提取请求参数
        # goal_handle.request 包含客户端发送的所有请求数据
        order = goal_handle.request.order

        # 初始化反馈消息对象
        # Feedback 对象用于向客户端发送进度信息
        feedback_msg = Fibonacci.Feedback()

        # 初始化斐波那契数列的前两个数
        # partial_sequence 字段将包含当前已计算的部分序列
        feedback_msg.partial_sequence = [0, 1]

        # 主计算循环：逐步计算斐波那契数列
        # 从索引 1 开始，因为前两个数已经初始化
        for i in range(1, order):

            # ==================== 取消检查 ====================
            # 这是响应性设计的关键：每次迭代都检查是否有取消请求
            if goal_handle.is_cancel_requested:
                # 如果检测到取消请求，立即执行取消操作
                goal_handle.canceled()  # 标记目标为已取消状态
                self.get_logger().info('目标被取消')

                # 返回空的结果对象
                # 注意：即使被取消，也需要返回一个 Result 对象
                return Fibonacci.Result()

            # ==================== 核心计算逻辑 ====================
            # 计算下一个斐波那契数：F(n) = F(n-1) + F(n-2)
            # partial_sequence[i] 是当前最新的数
            # partial_sequence[i-1] 是前一个数
            next_fib = feedback_msg.partial_sequence[i] + feedback_msg.partial_sequence[i - 1]

            # 将新计算的数添加到序列中
            feedback_msg.partial_sequence.append(next_fib)

            # 记录当前计算进度（便于调试和监控）
            self.get_logger().info(f'当前序列: {feedback_msg.partial_sequence}')

            # ==================== 反馈发送 ====================
            # 向客户端发送当前的计算进度
            # 这是 Action 系统的核心特性：实时进度反馈
            goal_handle.publish_feedback(feedback_msg)

            # ==================== 模拟计算时间 ====================
            # 暂停 0.5 秒来模拟实际的计算耗时
            # 在真实应用中，这里会是实际的复杂计算逻辑
            # 这个延迟也给取消检查提供了合理的响应时间
            time.sleep(0.5)

        # ==================== 任务完成处理 ====================
        # 标记目标为成功完成状态
        # 这会通知客户端任务已经完成
        goal_handle.succeed()

        # 创建并填充结果对象
        result = Fibonacci.Result()
        # 将完整的斐波那契序列作为最终结果
        result.sequence = feedback_msg.partial_sequence

        # 记录任务完成信息
        self.get_logger().info(f'任务完成，结果: {result.sequence}')

        # 返回结果给客户端
        return result


# ==================== 程序入口点 ====================
def main(args=None):
    """
    主函数：管理整个程序的生命周期

    参数：
        args: 命令行参数，默认为 None

    程序生命周期管理：
    1. 初始化 ROS2 系统
    2. 创建并启动节点
    3. 进入事件循环
    4. 优雅关闭和资源清理
    """

    # ==================== ROS2 系统初始化 ====================
    # 初始化 ROS2 通信系统
    # 这必须在创建任何 ROS2 对象之前调用
    # args 参数允许传递命令行参数给 ROS2 系统
    rclpy.init(args=args)

    # ==================== 节点创建和启动 ====================
    # 创建 FibonacciActionServer 实例
    # 此时 Action 服务器已经启动并开始监听客户端请求
    node = FibonacciActionServer()

    try:
        # ==================== 进入事件循环 ====================
        # rclpy.spin() 启动事件循环，使节点能够：
        # 1. 接收和处理来自客户端的请求
        # 2. 执行定时器回调
        # 3. 处理各种 ROS2 通信事件
        # 4. 维持程序运行直到被中断
        rclpy.spin(node)

    except KeyboardInterrupt:
        # ==================== 中断处理 ====================
        # 捕获 Ctrl+C 中断信号
        # 使用 pass 表示优雅地忽略中断，不打印错误信息
        # 这是处理用户主动终止程序的标准方式
        pass

    # ==================== 资源清理 ====================
    # 销毁节点对象，释放相关资源
    # 这包括关闭网络连接、清理内存等
    node.destroy_node()

    # 关闭 ROS2 系统
    # 这会清理所有 ROS2 相关的全局资源
    rclpy.shutdown()


# ==================== 脚本执行入口 ====================
if __name__ == '__main__':
    """
    Python 脚本的标准执行入口

    当此文件被直接执行时（而不是被导入），会调用 main() 函数
    这种模式允许代码既可以作为独立程序运行，也可以作为模块被其他代码导入
    """
    main()